#include "AccelStepper.h"
#include "PolargraphLib.h"
#include "EEPROMAnything.h"
#include "HardwareSerial.h"
#include "Servo.h"

//  EEPROM addresses
#define EEPROM_MACHINE_WIDTH			0
#define EEPROM_MACHINE_HEIGHT			2
#define	EEPROM_MACHINE_NAME				4
#define	EEPROM_MACHINE_MM_PER_REV		14
#define	EEPROM_MACHINE_STEPS_PER_REV	16

// Default machine definition
#define DEFAULT_MACHINE_NAME 			"PG01    "
#define DEFAULT_MACHINE_WIDTH 			650
#define DEFAULT_MACHINE_HEIGHT			650
#define DEFAULT_MM_PER_REV				95
#define DEFAULT_STEPS_PER_REV			800

#define BAUD_RATE 57600
#define COMMA ","
#define FREE_MEMORY_STRING "MEMORY,"
#define OUT_CMD_CARTESIAN "CARTESIAN,"
#define OUT_CMD_SYNC "SYNC,"
#define CMD_END ",END"

#define PEN_HEIGHT_UP_POSITION 180
#define PEN_HEIGHT_DOWN_POSITION 90
#define PEN_HEIGHT_SERVO_PIN 10
#define PEN_HEIGHT_CHANGE_DELAY 10



// *** private functions *****************************************************
void Polargraph::loadMachineSpecFromEeprom()
{
  
  unsigned int w = this->EEPROMReadInt(EEPROM_MACHINE_WIDTH);
  this->_widthMm = w;
  
  
  if (this->_widthMm < 1)
  {
    this->_widthMm = DEFAULT_MACHINE_WIDTH;
  }
  Serial.print(F("Loaded machine width:"));
  Serial.println(this->_widthMm);
  
  this->_widthMm = this->EEPROMReadInt(EEPROM_MACHINE_HEIGHT);
  if (this->_heightMm < 1)
  {
    this->_heightMm = DEFAULT_MACHINE_HEIGHT;
  }
  Serial.print(F("Loaded machine height:"));
  Serial.println(this->_heightMm);

  this->_mmPerRev = this->EEPROMReadInt(EEPROM_MACHINE_MM_PER_REV);
  if (this->_mmPerRev < 1)
  {
    this->_mmPerRev = DEFAULT_MM_PER_REV;
  }
  Serial.print(F("Loaded mm per rev:"));
  Serial.println(this->_mmPerRev);

  this->_stepsPerRev = this->EEPROMReadInt(EEPROM_MACHINE_STEPS_PER_REV);
  if (this->_stepsPerRev < 1)
  {
    this->_stepsPerRev = DEFAULT_STEPS_PER_REV;
  }
  Serial.print(F("Loaded motor steps per rev:"));
  Serial.println(this->_stepsPerRev);

  this->_mmPerStep = this->_mmPerRev / this->_stepsPerRev;
  this->_stepsPerMm = this->_stepsPerRev / this->_mmPerRev;
  
  Serial.print(F("Recalculated mmPerStep ("));
  Serial.print(this->_mmPerStep);
  Serial.print(F(") and stepsPerMm ("));
  Serial.print(this->_stepsPerMm);
  Serial.print(F(")"));
  Serial.println();
  
  this->_width = this->_widthMm * this->_stepsPerMm;
  Serial.print(F("Recalculated pageWidth in steps ("));
  Serial.print(this->_width);
  Serial.print(F(")"));
  Serial.println();
  this->_height = this->_heightMm * this->_stepsPerMm;
  Serial.print(F("Recalculated pageHeight in steps ("));
  Serial.print(this->_height);
  Serial.print(F(")"));
  Serial.println();


  String name = "        ";
  EEPROM_readAnything(EEPROM_MACHINE_NAME, name);
  
  if (name[0] == 0)
    name = DEFAULT_MACHINE_NAME;
  
  this->_name = name;
  _Serial->print(F("Loaded machine name:"));
  _Serial->println(this->_name);
}

/*  ==============================================================
    Writing and reading from non-volatile EEPROM 
=================================================================*/

//This function will write a 2 byte integer to the eeprom at the specified address and address + 1
void Polargraph::EEPROMWriteInt(int p_address, int p_value)
{
  this->_Serial->print(F("Writing Int "));
  this->_Serial->print(p_value);
  this->_Serial->print(F(" to address "));
  this->_Serial->println(p_address);


  EEPROM_writeAnything(p_address, p_value);
}

//This function will read a 2 byte integer from the eeprom at the specified address and address + 1
unsigned int Polargraph::EEPROMReadInt(int p_address)
{
  int value = 0;
  EEPROM_readAnything(p_address, value);
  return value;
}


// *** protected functions *****************************************************


// *** public functions *****************************************************

Polargraph::Polargraph(AccelStepper motorA, AccelStepper motorB)
{
	this->_motorA = motorA;
	this->_motorB = motorB;
	init();
}

AccelStepper Polargraph::getMotorA()
{
	return this->_motorA;
}
AccelStepper Polargraph::getMotorB()
{
	return this->_motorB;
}
void Polargraph::init()
{
	loadMachineSpecFromEeprom();
	this->_mmPerStep = this->getMmPerRev() / this->getStepsPerRev();
	this->_stepsPerMm = this->getStepsPerRev() / this->getMmPerRev();
	this->lastWaveWasTop = true;
	this->lastMotorBiasWasA = true;
	this->globalDrawDirection = DIR_NW;
	this->globalDrawDirectionMode = DIR_MODE_AUTO;
	this->currentlyRunning = true;
	this->usingAcceleration = true;
	this->setMotorAcceleration(this->_motorAcceleration);
	this->setMotorMaxSpeed(this->_motorMaxSpeed);
	this->movePenUp();
}
void Polargraph::resetEeprom()
{
  for (int i = 0; i <20; i++)
  {
    EEPROM.write(i, 0);
  }
  loadMachineSpecFromEeprom();
}
void Polargraph::dumpEeprom()
{
  if (_Serial != NULL)
  {
	  for (int i = 0; i <20; i++)
	  {
		_Serial->print(i);
		_Serial->print(". ");
		_Serial->println(EEPROM.read(i));
	  }
  }
}
float Polargraph::getMmPerRev()
{
	return this->_mmPerRev;
}
void Polargraph::setMmPerRev(float mmPerRev)
{
	EEPROMWriteInt(EEPROM_MACHINE_MM_PER_REV, mmPerRev);
	init();
}
float Polargraph::getStepsPerRev()
{
	return this->_stepsPerRev;
}
void Polargraph::setStepsPerRev(float stepsPerRev)
{
	EEPROMWriteInt(EEPROM_MACHINE_STEPS_PER_REV, stepsPerRev);
	init();
}
float Polargraph::getHeightMm()
{
	return this->_heightMm;
}
void Polargraph::setHeightMm(float size)
{
	EEPROMWriteInt(EEPROM_MACHINE_WIDTH, size);
	init();
}
float Polargraph::getWidthMm()
{
	return this->_widthMm;
}
void Polargraph::setWidthMm(float size)
{
	EEPROMWriteInt(EEPROM_MACHINE_HEIGHT, size);
	init();
}
void Polargraph::setName(String nameStr)
{
	char name[9] = "        ";
	if (name != DEFAULT_MACHINE_NAME)
	{
		for (byte i = 0; i < nameStr.length(); i++)
		{
			name[i] = nameStr[i];
		}
		EEPROM_writeAnything(EEPROM_MACHINE_NAME, name);
	}
	init();
}


void Polargraph::drawSquareWaveAlongA(int waveAmplitude, int waveLength, int totalWaves, int waveNo)
{
  if (waveNo == 0) 
  { 
    // first one, half a line and an along
//    Serial.println("First wave half");
    if (lastWaveWasTop) {
      moveB(waveAmplitude/2);
      moveA(waveLength);
    }
    else {
      moveB(0-(waveAmplitude/2));
      moveA(waveLength);
    }
    flipWaveDirection();
  }
  else if (waveNo == totalWaves) 
  { 
    // last one, half a line with no along
    if (lastWaveWasTop) {
      moveB(waveAmplitude/2);
    }
    else {
      moveB(0-(waveAmplitude/2));
    }
  }
  else 
  { 
    // intervening lines - full lines, and an along
    if (lastWaveWasTop) {
      moveB(waveAmplitude);
      moveA(waveLength);
    }
    else {
      moveB(0-waveAmplitude);
      moveA(waveLength);
    }
    flipWaveDirection();
  }
}

void Polargraph::drawSquareWaveAlongB(int waveAmplitude, int waveLength, int totalWaves, int waveNo)
{
  if (waveNo == 0) 
  { 
    // first one, half a line and an along
    if (lastWaveWasTop) {
      moveA(waveAmplitude/2);
      moveB(waveLength);
    }
    else {
      moveA(0-(waveAmplitude/2));
      moveB(waveLength);
    }
    flipWaveDirection();
  }
  else if (waveNo == totalWaves) 
  { 
    // last one, half a line with no along
    if (lastWaveWasTop) {
      moveA(waveAmplitude/2);
    }
    else {
      moveA(0-(waveAmplitude/2));
    }
  }
  else 
  { 
    // intervening lines - full lines, and an along
    if (lastWaveWasTop) {
      moveA(waveAmplitude);
      moveB(waveLength);
    }
    else {
      moveA(0-waveAmplitude);
      moveB(waveLength);
    }
    flipWaveDirection();
  }
}


void Polargraph::flipWaveDirection()
{
  if (lastWaveWasTop)
    lastWaveWasTop = false;
  else
    lastWaveWasTop = true;
}
void Polargraph::moveA(long dist)
{
  this->_motorA.move(dist);
  while (this->_motorA.distanceToGo() != 0)
  {
    if (this->currentlyRunning)
      this->_motorA.run();
  }
}

void Polargraph::moveB(long dist)
{
  this->_motorB.move(dist);
  while (this->_motorB.distanceToGo() != 0)
  {
    if (this->currentlyRunning)
      this->_motorB.run();
  }
}


void Polargraph::changeLength(long tA, long tB)
{
  this->_motorA.moveTo(tA);
  this->_motorB.moveTo(tB);
  
  while (this->_motorA.distanceToGo() != 0 || this->_motorB.distanceToGo() != 0)
  {
    if (this->currentlyRunning)
    {
      if (this->usingAcceleration)
      {
        this->_motorA.run();
        this->_motorB.run();
      }
      else
      {
        this->_motorA.runSpeedToPosition();
        this->_motorB.runSpeedToPosition();
      }
    }
  }
  
  reportPosition();
}


long Polargraph::getMaxLength()
{
  if (this->_maxLength == 0)
  {
    float length = this->getMachineA(this->_width * this->_stepsPerMm, this->_height * this->_stepsPerMm);
    this->_maxLength = long(length+0.5);
  }
  return this->_maxLength;
}


void Polargraph::changeLengthDirect(float endA, float endB, int maxSegmentLength)
{
  
  float startA = this->_motorA.currentPosition();
  float startB = this->_motorB.currentPosition();
//  Serial.println(F("Drawing direct line"));

  if (endA < 20 || endB < 20 || endA > this->getMaxLength() | endB > this->getMaxLength())
  {
    Serial.println("This point falls outside the area of this machine. Skipping it.");
  }
  else
  {
    drawBetweenPoints(startA, startB, endA, endB, maxSegmentLength);
  }
}  

/**
Thanks to Andy Kinsman for help with this method.

This moves the gondola in a straight line between p1 and p2.  Both input coordinates are in 
the native coordinates system.  

The fidelity of the line is controlled by maxLength - this is the longest size a line segment is 
allowed to be.  1 is finest, slowest.  Use higher values for faster, wobblier.
*/
void Polargraph::drawBetweenPoints(float p1a, float p1b, float p2a, float p2b, int maxSegmentLength)
{
  // ok, we're going to plot some dots between p1 and p2.  Using maths. I know! Brave new world etc.
  
  this->reportingPosition = false;
  
  // First, convert these values to cartesian coordinates
  // We're going to figure out how many segments the line
  // needs chopping into.
  float c1x = this->getCartesianXFP(p1a, p1b);
  float c1y = this->getCartesianYFP(c1x, p1a);
  
  float c2x = this->getCartesianXFP(p2a, p2b);
  float c2y = this->getCartesianYFP(c2x, p2a);
  
  // test to see if it's on the page
  if (c2x > 20 && c2x<this->_width-20 && c2y > 20 && c2y <this->_height-20)
  {
    float deltaX = c2x-c1x;    // distance each must move (signed)
    float deltaY = c2y-c1y;
  
    int linesegs = 1;            // assume at least 1 line segment will get us there.
    if (abs(deltaX) > abs(deltaY))
    {
      // slope <=1 case    
      while ((abs(deltaX)/linesegs) > maxSegmentLength)
      {
        linesegs++;
      }
    }
    else
    {
      // slope >1 case
      while ((abs(deltaY)/linesegs) > maxSegmentLength)
      {
        linesegs++;
      }
    }
    
    // reduce delta to one line segments' worth.
    deltaX = deltaX/linesegs;
    deltaY = deltaY/linesegs;
  
    // render the line in N shorter segments
    while (linesegs > 0)
    {
      // compute next new location
      c1x = c1x + deltaX;
      c1y = c1y + deltaY;
  
      // convert back to machine space
      float pA = this->getMachineA(c1x, c1y);
      float pB = this->getMachineB(c1x, c1y);
    
      // do the move
      useAcceleration(false);
      changeLength(pA, pB);
  
      // one line less to do!
      linesegs--;
    }
    
    // do the end point in case theres been some rounding errors etc
    this->reportingPosition = true;
    changeLength(p2a, p2b);
    useAcceleration(true);
  }
  else
  {
    Serial.println("Line is not on the page. Skipping it.");
  }
  this->outputAvailableMemory();
}

void Polargraph::useAcceleration(boolean use)
{
  this->usingAcceleration = use;
}

/* AS220 Drawbot */
float Polargraph::rads(int n) {
  // Return an angle in radians
  return (n/180.0 * PI);
}    

void Polargraph::drawCurve(long x, long y, long fx, long fy, long cx, long cy) {
  // Draw a Quadratic Bezier curve from (x, y) to (fx, fy) using control pt
  // (cx, cy)
  float xt=0;
  float yt=0;

  for (float t=0; t<=1; t+=.0025) {
    xt = pow((1-t),2) *x + 2*t*(1-t)*cx+ pow(t,2)*fx;
    yt = pow((1-t),2) *y + 2*t*(1-t)*cy+ pow(t,2)*fy;
    changeLength(xt, yt);
  }  
}
                                                     

void Polargraph::drawCircleAtPosition(long centerx, long centery, int radius) 
{
  // Estimate a circle using 20 arc Bezier curve segments
  int segments =20;
  int angle1 = 0;
  int midpoint=0;
   
//  changeLength(centerx+radius, centery);

  for (float angle2=360/segments; angle2<=360; angle2+=360/segments) {

    midpoint = angle1+(angle2-angle1)/2;

    float startx=centerx+radius*cos(rads(angle1));
    float starty=centery+radius*sin(rads(angle1));
    float endx=centerx+radius*cos(rads(angle2));
    float endy=centery+radius*sin(rads(angle2));
    
    int t1 = rads(angle1)*1000 ;
    int t2 = rads(angle2)*1000;
    int t3 = angle1;
    int t4 = angle2;

    drawCurve(startx,starty,endx,endy,
              centerx+2*(radius*cos(rads(midpoint))-.25*(radius*cos(rads(angle1)))-.25*(radius*cos(rads(angle2)))),
              centery+2*(radius*sin(rads(midpoint))-.25*(radius*sin(rads(angle1)))-.25*(radius*sin(rads(angle2))))
    );
    
    angle1=angle2;
  }
}

void Polargraph::drawSpiralAtPosition(long centerx, long centery, int size, int brightness) 
{
	this->_Serial->print("Brightness before: ");
	this->_Serial->println(brightness);
     
    int density = this->scaleDensity(brightness, 255, 8);
    this->_Serial->print("Density scaled");
    this->_Serial->println(density);
    
    int radius = size / 2;
    int increment = radius / 8;

    if (density > 0)
    {
      this->_Serial->print("Density:");
      this->_Serial->println(density);
	  // move to start point
      this->drawSpiral(radius, increment, density);
    }
    this->outputAvailableMemory();
}

void Polargraph::drawSpiral(int maxRadius, int gap, int density)
{
  this->_Serial->println("Draw spiral.");
  this->_Serial->print("Max radius: ");
  this->_Serial->println(maxRadius);
  this->_Serial->print("Gap:");
  this->_Serial->println(gap);
  this->_Serial->print("densitY:");
  this->_Serial->println(density);
  
  // Estimate a circle using 20 arc Bezier curve segments
  int segments = 20;

  
  float radius = float(gap);
  // work out how many shells to draw
  for (int turns = 0; turns < density; turns++)
  {
    if (gap < 80)
      segments = radius / 8;
    if (segments < 4)
      segments == 4;

    float anglePerSegment = 360/segments;
    
    float radiusIncrementPerSegment = float(gap) / float(segments);
  
    int angle1 = 0;
    int midpoint = 0;
    boolean firstMove = true;
	long cartesianPosition[2];
	this->getCartesian(cartesianPosition);
	long centerx = cartesianPosition[0];
	long centery = cartesianPosition[1];
    for (float angle2=anglePerSegment; angle2<=360; angle2+=anglePerSegment) 
    {
      midpoint = angle1+(angle2-angle1)/2;
  
      float startx=centerx+radius*cos(rads(angle1));
      float starty=centery+radius*sin(rads(angle1));
      float endx=centerx+radius*cos(rads(angle2));
      float endy=centery+radius*sin(rads(angle2));
      
      int t1 = rads(angle1)*1000 ;
      int t2 = rads(angle2)*1000;
      int t3 = angle1;
      int t4 = angle2;
  
      if (firstMove)
      {
        this->changeLength(startx, starty);
        firstMove = false;
      }
//      useAcceleration(false);
      drawCurve(startx,starty,endx,endy,
                centerx+2*(radius*cos(rads(midpoint))-.25*(radius*cos(rads(angle1)))-.25*(radius*cos(rads(angle2)))),
                centery+2*(radius*sin(rads(midpoint))-.25*(radius*sin(rads(angle1)))-.25*(radius*sin(rads(angle2))))
      );
//      useAcceleration(true);
      angle1=angle2;
      radius += radiusIncrementPerSegment;
    }
  }
}

//  PIXEL TYPES

void Polargraph::drawSquarePixelAtPosition(long originA, long originB, int size, int density)
{

	// work out how many waves to draw
    int waves = scaleDensity(density, 255, maxDensity(this->_penWidth, size));
	if (waves == 0)
	{
		/* Zero waves, this is an empty pixel.
		 So.  If the pen is up, then I'm not going to bother moving at all.
		 What difference will it make? None. Exactly.
		 */
		 if (this->_penRaised)
		 {
			// do nothing for this pixel
		 }
		 else
		 {
			// otherwise, let's get moving
			int halfSize = size / 2;

			long startPointA;
			long startPointB;
			long endPointA;
			long endPointB;

			int calcFullSize = halfSize * 2; // see if there's any rounding errors
			int offsetStart = size - calcFullSize;

			// work out the draw direction if it needs to change.
			if (this->_globalDrawDirectionMode == DIR_MODE_AUTO)
			{
				this->_globalDrawDirection = getAutoDrawDirection(originA, originB, this->getMotorA().currentPosition(), this->getMotorB().currentPosition());
			}

			// calculate start and end points, given the origin and the drawing direction
			if (this->_globalDrawDirection == DIR_SE) 
			{
			//      Serial.println(F("d: SE"));
				startPointA = originA - halfSize;
				startPointA += offsetStart;
				startPointB = originB;
				endPointA = originA + halfSize;
				endPointB = originB;
			}
			else if (this->_globalDrawDirection == DIR_SW)
			{
			//      Serial.println(F("d: SW"));
				startPointA = originA;
				startPointB = originB - halfSize;
				startPointB += offsetStart;
				endPointA = originA;
				endPointB = originB + halfSize;
			}
			else if (this->_globalDrawDirection == DIR_NW)
			{
			//      Serial.println(F("d: NW"));
				startPointA = originA + halfSize;
				startPointA -= offsetStart;
				startPointB = originB;
				endPointA = originA - halfSize;
				endPointB = originB;
			}
			else //(drawDirection == DIR_NE)
			{
			//      Serial.println(F("d: NE"));
				startPointA = originA;
				startPointB = originB + halfSize;
				startPointB -= offsetStart;
				endPointA = originA;
				endPointB = originB - halfSize;
			}

			this->_Serial->print(F("Start point: "));
			this->_Serial->print(startPointA);
			this->_Serial->print(COMMA);
			this->_Serial->print(startPointB);
			this->_Serial->print(F(". end point: "));
			this->_Serial->print(endPointA);
			this->_Serial->print(COMMA);
			this->_Serial->print(endPointB);
			this->_Serial->println(F("."));

			// move to start point
			this->changeLength(startPointA, startPointB);

			// shade the pixel
			if (waves > 1)
			{
				this->drawSquarePixel(size, size, waves, this->_globalDrawDirection);
			}
	
			// move to end point
			this->changeLength(endPointA, endPointB);
			this->outputAvailableMemory(); 
		 }
	}
}

void Polargraph::drawSquarePixel(int length, int width, int density, byte drawDirection) 
{

  // work out how wide each segment should be
  int segmentLength = 0;

  if (density > 0)
  {
    // work out some segment widths
    int basicSegLength = length / density;
    int basicSegRemainder = length % density;
    float remainderPerSegment = float(basicSegRemainder) / float(density);
    float totalRemainder = 0.0;
    int lengthSoFar = 0;
    
//    Serial.print("Basic seg length:");
//    Serial.print(basicSegLength);
//    Serial.print(", basic seg remainder:");
//    Serial.print(basicSegRemainder);
//    Serial.print(", remainder per seg");
//    Serial.println(remainderPerSegment);
    
    for (int i = 0; i <= density; i++) 
    {
      totalRemainder += remainderPerSegment;

      if (totalRemainder >= 1.0)
      {
        totalRemainder -= 1.0;
        segmentLength = basicSegLength+1;
      }
      else
      {
        segmentLength = basicSegLength;
      }

      if (drawDirection == DIR_SE) {
        this->drawSquareWaveAlongA(width, segmentLength, density, i);
      }
      if (drawDirection == DIR_SW) {
        this->drawSquareWaveAlongB(width, segmentLength, density, i);
      }
      if (drawDirection == DIR_NW) {
        segmentLength = 0 - segmentLength; // reverse
        this->drawSquareWaveAlongA(width, segmentLength, density, i);
      }
      if (drawDirection == DIR_NE) {
        segmentLength = 0 - segmentLength; // reverse
        this->drawSquareWaveAlongB(width, segmentLength, density, i);
      }
      lengthSoFar += segmentLength;
    //      Serial.print("distance so far:");
    //      Serial.print(distanceSoFar);
      
      
      this->reportPosition();
    } // end of loop
  }
}



void Polargraph::drawScribblePixelAtPosition(long originA, long originB, int pixelSize, int brightness, int swell) 
{
	// scribble works best when it's slightly oversized
	int expandedPixelSize = pixelSize * 1.1;
	
	// convert brightness to density
    int maxDens = this->maxDensity(this->_penWidth, pixelSize);
    int density = scaleDensity(density, 255, maxDens);


//  int originA = accelA.currentPosition();
//  int originB = accelB.currentPosition();
  
  long lowLimitA = originA-(expandedPixelSize/2);
  long highLimitA = lowLimitA+expandedPixelSize;
  long lowLimitB = originB-(expandedPixelSize/2);
  long highLimitB = lowLimitB+expandedPixelSize;
  int randA;
  int randB;
  
  int currSize = expandedPixelSize;
  
  for (int i = 0; i <= density; i++)
  {
    randA = random(0, currSize);
    randB = random(0, currSize);
    changeLength(lowLimitA+randA, lowLimitB+randB);

	// Look, after each line, there's the opportunity to increase the available area to
	// draw in.  This blurs the edge of the pixel.
	// If swell is 0 then it makes no difference.
    lowLimitA-=swell;
    highLimitA+=swell;
    currSize+=swell*2;
  }
}

void Polargraph::reportPosition()
{
  if (this->reportingPosition && _Serial != NULL)
  {
    _Serial->print(OUT_CMD_SYNC);
    _Serial->print(this->_motorA.currentPosition());
    _Serial->print(COMMA);
    _Serial->print(this->_motorB.currentPosition());
    _Serial->println(CMD_END);
    
  //  int cX = this->getCartesianX();
  //  int cY = this->getCartesianY(cX, this->motorA.currentPosition());
  //  _Serial.print(OUT_CMD_CARTESIAN);
  //  _Serial.print(cX*mmPerStep);
  //  _Serial.print(COMMA);
  //  _Serial.print(cY*mmPerStep);
  //  _Serial.println(CMD_END);
//    outputAvailableMemory();
  }
}

void Polargraph::outputAvailableMemory()
{
  if (_Serial != NULL)
  {
	  int avMem = availableMemory();
	  if (avMem != _availMem)
	  {
		_availMem = avMem;
		_Serial->print(FREE_MEMORY_STRING);
		_Serial->print(_availMem);
		_Serial->println(CMD_END);
	  }
  }
}

//from http://www.arduino.cc/playground/Code/AvailableMemory
unsigned int Polargraph::availableMemory() 
{
  uint8_t * heapptr, * stackptr;
  stackptr = (uint8_t *)malloc(4);
  heapptr = stackptr;
  free(stackptr);               
  stackptr = (uint8_t *)(SP);
  return stackptr - heapptr;
} 

void Polargraph::setSerial(HardwareSerial *serIn)
{
	_Serial = serIn;
	_Serial->begin(BAUD_RATE);
}

void Polargraph::setPosition(long pA, long pB)
{
	this->_motorA.setCurrentPosition(pA);
	this->_motorB.setCurrentPosition(pB);
}

void Polargraph::engageMotors()
{
  this->_motorA.runToNewPosition(_motorA.currentPosition()+4);
  this->_motorB.runToNewPosition(_motorB.currentPosition()+4);
  this->_motorA.runToNewPosition(_motorA.currentPosition()-4);
  this->_motorB.runToNewPosition(_motorB.currentPosition()-4);
  _Serial->println("Engaged motors.");
}

void Polargraph::releaseMotors()
{
  //penUp();
  this->_motorA.disableOutputs();
  this->_motorB.disableOutputs();
}


float Polargraph::getMachineA(float cX, float cY)
{
  float a = sqrt(sq(cX)+sq(cY));
  return a;
}
float Polargraph::getMachineB(float cX, float cY)
{
  float b = sqrt(sq((_width)-cX)+sq(cY));
  return b;
}

float Polargraph::getCartesianXFP(float aPos, float bPos)
{
  float calcX = (pow(_width, 2) - pow(bPos, 2) + pow(aPos, 2)) / (_width*2);
  return calcX;  
}
float Polargraph::getCartesianYFP(float cX, float aPos) 
{
  float calcY = sqrt(pow(aPos,2)-pow(cX,2));
  return calcY;
}


int Polargraph::getCartesianX(float aPos, float bPos)
{
  int calcX = int((pow(_width, 2) - pow(bPos, 2) + pow(aPos, 2)) / (_width*2));
  return calcX;  
}

int Polargraph::getCartesianX() {
  int calcX = getCartesianX(_motorA.currentPosition(), _motorB.currentPosition());
  return calcX;  
}

int Polargraph::getCartesianY() {
  return getCartesianY(getCartesianX(), _motorA.currentPosition());
}
int Polargraph::getCartesianY(int cX, float aPos) {
  int calcX = cX;
  int calcY = int(sqrt(pow(aPos,2)-pow(calcX,2)));
  return calcY;
}

void Polargraph::getCartesian(long cartesianCoords[2])
{
	long cX = long((pow(_width, 2) - pow(this->getMotorB().currentPosition(), 2) + pow(this->getMotorA().currentPosition(), 2)) / (_width*2));
	long cY = long(sqrt(pow(this->getMotorA().currentPosition(), 2)-pow(cX,2)));
	cartesianCoords[0] = cX;
	cartesianCoords[1] = cY;
}

void Polargraph::setMotorMaxSpeed(float speed)
{
  this->_motorMaxSpeed = speed;
  this->_motorA.setMaxSpeed(this->_motorMaxSpeed);
  this->_motorB.setMaxSpeed(this->_motorMaxSpeed);
  Serial.print(F("New max speed: "));
  Serial.println(this->_motorMaxSpeed);
}
float Polargraph::getMotorMaxSpeed()
{
	return this->_motorMaxSpeed;
}

void Polargraph::setMotorAcceleration(float accel)
{
  this->_motorAcceleration = accel;
  this->_motorA.setAcceleration(this->_motorAcceleration);
  this->_motorB.setAcceleration(this->_motorAcceleration);
  Serial.print(F("New acceleration: "));
  Serial.println(this->_motorAcceleration);
}
float Polargraph::getMotorAcceleration()
{
	return this->_motorAcceleration;
}

void Polargraph::setPenWidth(float penWidth)
{
	this->_penWidth = penWidth;
	this->_Serial->print(F("Changed Pen width to "));
	this->_Serial->print(this->_penWidth);
	this->_Serial->print(F("mm"));
	this->_Serial->println();
}   
float Polargraph::getPenWidth()
{
	return this->_penWidth;
}

bool Polargraph::getPenRaised()
{
	return this->_penRaised;
}
Servo Polargraph::getPenLiftServo()
{
	return this->_penHeightServo;
}

void Polargraph::penUp()
{
	this->_Serial->println("Pen up.");
	if (!this->_penRaised)
	{
		// if pen isn't already up, then raise it.
		movePenUp();
	}
}

void Polargraph::movePenUp()
{
	this->getPenLiftServo().attach(PEN_HEIGHT_SERVO_PIN);
	for (int i=PEN_HEIGHT_DOWN_POSITION; i<PEN_HEIGHT_UP_POSITION; i++) 
	{
		this->getPenLiftServo().write(i);
		delay(PEN_HEIGHT_CHANGE_DELAY);
	}
	this->getPenLiftServo().detach();
	_penRaised = true;
}  


void Polargraph::penDown()
{
	this->_Serial->println("Pen down.");
	if (this->_penRaised)
	{
		// if pen is up, then move it down
		this->movePenDown();
	}
}
void Polargraph::movePenDown()
{
	this->getPenLiftServo().attach(PEN_HEIGHT_SERVO_PIN);
	for (int i=PEN_HEIGHT_UP_POSITION; i>PEN_HEIGHT_DOWN_POSITION; i--) 
	{
		this->getPenLiftServo().write(i);
		delay(PEN_HEIGHT_CHANGE_DELAY);
	}
	this->getPenLiftServo().detach();
	_penRaised = false;
}
void Polargraph::testPenHeight()
{
  delay(3000);
  this->penUp();
  delay(3000);
  this->penDown();
  delay(3000);
}
void Polargraph::testServoRange()
{
  this->getPenLiftServo().attach(PEN_HEIGHT_SERVO_PIN);
  for (int i=0; i<200; i++) {
    this->_Serial->println(i);
    this->getPenLiftServo().write(i);
    delay(PEN_HEIGHT_CHANGE_DELAY);
  }
  this->getPenLiftServo().detach();
  
}


int Polargraph::minSegmentSizeForPen(float penSize)
{
  float penSizeInSteps = penSize * this->_stepsPerMm;

  int minSegSize = 1;
  if (penSizeInSteps >= 2.0)
    minSegSize = int(penSizeInSteps);
    
//  Serial.print(F("Min segment size for penSize "));
//  Serial.print(penSize);
//  Serial.print(F(": "));
//  Serial.print(minSegSize);
//  Serial.print(F(" steps."));
//  Serial.println();
  
  return minSegSize;
}

int Polargraph::maxDensity(float penSize, int rowSize)
{
  float rowSizeInMm = this->_mmPerStep * rowSize;
//  Serial.print(F("rowsize in mm: "));
//  Serial.print(rowSizeInMM);
//  Serial.print(F(", mmPerStep: "));
//  Serial.print(mmPerStep);
//  Serial.print(F(", rowsize: "));
//  Serial.println(rowSize);
  
  float numberOfSegments = rowSizeInMm / penSize;
  int maxDens = 1;
  if (numberOfSegments >= 2.0)
    maxDens = int(numberOfSegments);
    
//  Serial.print("num of segments float:");
//  Serial.println(numberOfSegments);
//
//    
//  Serial.print(F("Max density: penSize: "));
//  Serial.print(penSize);
//  Serial.print(F(", rowSize: "));
//  Serial.print(rowSize);
//  Serial.println(maxDens);
  
  return maxDens;
}

int Polargraph::scaleDensity(int inDens, int inMax, int outMax)
{
  float reducedDens = (float(inDens) / float(inMax)) * float(outMax);
  reducedDens = outMax-reducedDens;
//  Serial.print(F("inDens:"));
//  Serial.print(inDens);
//  Serial.print(F(", inMax:"));
//  Serial.print(inMax);
//  Serial.print(F(", outMax:"));
//  Serial.print(outMax);
//  Serial.print(F(", reduced:"));
//  Serial.println(reducedDens);
  
  // round up if bigger than .5
  int result = int(reducedDens);
  if (reducedDens - (result) > 0.5)
    result ++;
  
  return result;
}

  void Polargraph::testPenWidth(int rowWidth, float startWidth, float endWidth, float incSize)
  {
    int tempDirectionMode = this->_globalDrawDirectionMode;
    this->_globalDrawDirectionMode = DIR_MODE_PRESET;
    
    float oldPenWidth = this->_penWidth;
    int iterations = 0;
    
    for (float pw = startWidth; pw <= endWidth; pw+=incSize)
    {
      iterations++;
      this->_penWidth = pw;
      int maxDens = this->maxDensity(this->_penWidth, rowWidth);
      Serial.print(F("Penwidth test "));
      Serial.print(iterations);
      Serial.print(F(", pen width: "));
      Serial.print(this->_penWidth);
      Serial.print(F(", max density: "));
      Serial.println(maxDens);
      this->drawSquarePixel(rowWidth, rowWidth, maxDens, DIR_SE);
    }

    this->_penWidth = oldPenWidth;
    
    this->moveB(0-rowWidth);
    for (int i = 1; i <= iterations; i++)
    {
      this->moveB(0-(rowWidth/2));
      this->moveA(0-rowWidth);
      this->moveB(rowWidth/2);
    }
    
    this->_penWidth = oldPenWidth;
    this->_globalDrawDirectionMode = tempDirectionMode;
  }    

  void Polargraph::testPenWidthScribble(int rowWidth, float startWidth, float endWidth, float incSize, float swell)
  {
    boolean ltr = true;
    
    float oldPenWidth = this->_penWidth;
    int iterations = 0;
    
    int posA = this->getMotorA().currentPosition();
    int posB = this->getMotorB().currentPosition();

    int startColumn = posA;
    int startRow = posB;
    
    for (float pw = startWidth; pw <= endWidth; pw+=incSize)
    {
      iterations++;
      int column = posA;
      
      this->_penWidth = pw;
      int maxDens = this->maxDensity(this->_penWidth, rowWidth);
      this->_Serial->print(F("Penwidth test "));
      this->_Serial->print(iterations);
      this->_Serial->print(F(", pen width: "));
      this->_Serial->print(this->_penWidth);
      this->_Serial->print(F(", max density: "));
      this->_Serial->println(maxDens);
      
      for (int density = maxDens; density >= 0; density--)
      {
        this->drawScribblePixelAtPosition(posA, posB, rowWidth, density, swell);
        posB+=rowWidth;
      }
      
      posA+=rowWidth;
      posB = startRow;
    }
    
    this->changeLength(long(posA-(rowWidth/2)), long(startRow-(rowWidth/2)));

    this->_penWidth = oldPenWidth;
    
    this->moveB(0-rowWidth);
    for (int i = 1; i <= iterations; i++)
    {
      this->moveB(0-(rowWidth/2));
      this->moveA(0-rowWidth);
      this->moveB(rowWidth/2);
    }
    
    this->_penWidth = oldPenWidth;
  }    

  byte Polargraph::getRandomDrawDirection()
{
  return random(1, 5);
}

byte Polargraph::getAutoDrawDirection(long targetA, long targetB, long sourceA, long sourceB)
{
  byte dir = DIR_SE;

  if (targetA<sourceA && targetB<sourceA)
  {
//    Serial.println(F("calculated NW"));
    dir = DIR_NW;
  }
  else if (targetA>sourceA && targetB>sourceB)
  {
//    Serial.println(F("calculated SE"));
    dir = DIR_SE;
  }
  else if (targetA<sourceA && targetB>sourceB)
  {
//    Serial.println(F("calculated SW"));
    dir = DIR_SW;
  }
  else if (targetA>sourceA && targetB<sourceB)
  {
//    Serial.println(F("calculated NE"));
    dir = DIR_NE;
  }
  else if (targetA==sourceA && targetB<sourceB)
  {
//    Serial.println(F("calc NE"));
    dir = DIR_NE;
  }
  else if (targetA==sourceA && targetB>sourceB)
  {
//    Serial.println(F("calc SW"));
    dir = DIR_SW;
  }
  else if (targetA<sourceA && targetB==sourceB)
  {
//    Serial.println(F("calc NW"));
    dir = DIR_NW;
  }
  else if (targetA>sourceA && targetB==sourceB)
  {
//    Serial.println(F("calc SE"));
    dir = DIR_SE;
  }
  else
  {
//    Serial.println("Not calculated - default SE");
  }

  return dir;
}



